#include "control.h"

#include "handlostexception.h"
#include "gestureevent.h"
#include "handsmoveevent.h"
#include "statechangeevent.h"
#include "eventmanager.h"

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/date_time/microsec_time_clock.hpp>
#include <boost/thread/locks.hpp>

#define _USE_MATH_DEFINE
#include <math.h>

#include <iostream>
using namespace std;

using namespace mlgesture;

Control::Control(int video_id, int hands_num) throw(HandGestureException) :
	skinMorphingMethod_(SkinDetector::MORPHING_METHOD_DILATE_ERODE)
{
	isHand_ = false;
	isRunning_ = false;
	camId_ = video_id;

	if (hands_num < 1) {
		throw HandGestureException("Hands number must be a postitive");
	}
	dynamic_ = NULL;
	static_ = NULL;
	intersactions_ = NULL;
	setHandsNum(hands_num);
	frameSize_ = Size(500, 375);
	skinFilter_ = new SkinDetector(skinMorphingMethod_);
	handDetector_ = NULL;
	handsAngle_ = 0.0f;
	trackingSensity_ = 0.9;

	trajectoryActive_ = false;
	dynamicGestureActive_ = false;
	staticGestureActive_ = false;
	isError_ = false;
}

Control::~Control() {
	stopProcess();
	releaseCamera();
	if (handDetector_) {
		delete handDetector_;
	}
	delete skinFilter_;
	delete static_;
	delete dynamic_;
	clean();
}

void Control::clean() {
	for(size_t i=0; i<handTrackers_.size(); i++) {
		if (handTrackers_[i])
			delete handTrackers_[i];
	}
	handTrackers_.clear();
	deleteIntersactionTable();
}

struct lessRect : public binary_function<Rect, Rect, bool> {
	bool operator()(Rect x, Rect y) { return x.x < y.x; }
};

bool Control::process() {
	StateChangeEvent *state_event;
	HandsMoveEvent *move_event;

	try {
		isRunningMutex_.lock();
		bool state_tmp = isRunning_;
		isRunningMutex_.unlock();
		if (!state_tmp)
			return true;
		if (!cap_.isOpened()) {
			cap_.open(camId_);
		}

		if( !cap_.isOpened()) {
			throw HandGestureException("Could not initialize capturing..");
		}

		frameMutex_.lock();
		cap_ >> frame_;

		if( frame_.empty())
			throw HandGestureException("Could not capture a frame");

		isFrame_ = true;

		Mat tmp;
		resize(frame_,tmp,frameSize_,0,0, INTER_AREA);
		frame_ = tmp;
		flip(frame_, frame_,1);
		frameMutex_.unlock();

		if (bitMask_.data == NULL) {
			bitMask_ = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
		}
		if (handDetector_ == NULL) {
			handDetector_ = new HandDetector(frame_);
		}
		if(hsv_.data == NULL) {
			hsv_ = Mat(frame_.rows, frame_.cols, CV_8UC3);
		}

		cvtColor(frame_, hsv_, CV_BGR2HSV);
		split(hsv_, hsvChannels_);

		skinFilter_->process(frame_, bitMask_);
		if (!isHand_) {
			handDetector_->process(bitMask_);
			handsRect_ = handDetector_->getHands();
			if ((int)handsRect_.size() == handsNum_) {
				sort(handsRect_.begin(), handsRect_.end(), lessRect());
				Mat roi;
				for (int i=0; i<handsNum_; i++) {
					handTrackers_.push_back(new HandTracker());

					roi = Mat::zeros(frame_.rows, frame_.cols, CV_8UC1);
					rectangle(roi, handsRect_[i], Scalar(255), CV_FILLED);
					roi &= bitMask_;

					handTrackers_[i]->init(frame_, handsRect_[i], roi, trackingSensity_);
					createIntersactionTable(handsNum_);
				}
				if (handsNum_ == 2) {
					handRectL_ = handsRect_[0];
					handRectR_ = handsRect_[1];
				}
				state_event = new StateChangeEvent(true);
				EventManager::getInstance().sendEvent(state_event);
				delete state_event;
				isHand_ = true;
			}

		}

		if (isHand_) {
			try {
				for (int i=0; i<handsNum_; i++) {
					handsRect_[i] = handTrackers_[i]->track(frame_, bitMask_);
				}
				captureHandsShape();
				checkIntersactions();
				if (handsNum_ == 2) {
					handRectL_ = handsRect_[0];
					handRectR_ = handsRect_[1];

					int dx = handRectR_.x + handRectR_.width/2 - handRectL_.x - handRectL_.width/2;
					int dy = handRectR_.y + handRectR_.height/2 - handRectL_.y - handRectL_.height/2;
					handsDistance_ = sqrt((float)dx*dx+dy*dy);
					handsAngle_ = atan(1.*dy/dx);
				}
				centers_.clear();
				for (int i=0; i<handsNum_; i++) {
					Point pt(handsRect_[i].x + handsRect_[i].width/2, handsRect_[i].y + handsRect_[i].height/2);
					centers_.push_back(pt);
				}
				move_event = new HandsMoveEvent(centers_, handsRect_);
				EventManager::getInstance().sendEvent(move_event);
				delete move_event;

				if (trajectoryActive_) {
					processTrajectory();
				}
				if (dynamicGestureActive_) {
					processDynamicGestures();
				}
				if (staticGestureActive_) {
					proccessStaticGestures();
				}
			}
			catch (HandLostException &e) {
				clean();
				state_event = new StateChangeEvent(false);
				EventManager::getInstance().sendEvent(state_event);
				delete state_event;
				isHand_ = false;
			}
		}
		cvtColor(bitMask_>0, bitMaskBGR_, CV_GRAY2BGR);
	} catch(HandGestureException &e) {
		isError_ = true;
		errorMsg_ = e.what();
		return false;
	}
	return true;
}

void Control::getFrame(Mat &frame, vector<Rect> *hands) throw(HandGestureException) {
	if (isError_)
		throw HandGestureException(errorMsg_);
	if (!isFrame_) {
		frame = Mat();
		return;
	}

	if (isHand_ && hands!=NULL) {
		hands->clear();
		for (int i=0; i<handsNum_; i++) {
			hands->push_back(handsRect_[i]);
		}
	}
	boost::mutex::scoped_lock l(frameMutex_);
	frame = frame_.clone();
}

void Control::getMask(Mat &mask) const throw (HandGestureException) {
	if (isError_)
		throw HandGestureException(errorMsg_);
	mask = bitMaskBGR_.clone();
}

Size Control::getFrameSize() const {
	return frameSize_;
}

void Control::setHandsNum(const int num) throw (HandGestureException) {
	clean();
	handsNum_ = num;
	lastPos_.clear();
	lastDirection_.clear();
	trackingState_.clear();
	trajectoryEnd_.clear();
	trajectory_.clear();
	if (dynamic_) {
		delete dynamic_;
		dynamic_ = NULL;
	}
	if (static_) {
		delete static_;
		static_ = NULL;
	}
	for (int i=0; i<num; i++) {
		lastPos_.push_back(Point(-1,-1));
		lastDirection_.push_back(-1);
		trackingState_.push_back(false);
		trajectoryEnd_.push_back(false);
		trajectory_.push_back(Trajectory());

	}
	if (!dynamicGarnageFile_.isEmpty()) {
		dynamic_ = new DynamicGesture(dynamicModelFiles_, dynamicGarnageFile_, num);
	}
	if (!staticModelFile_.isEmpty()) {
		static_ = new StaticGesture(staticModelFile_, staticScaleFactorsFile_, staticLabelsFile_, num);
	}
}

void Control::adjustSkinFilter(const int lower_threshold, const int upper_threshold,
								   const int lower_value_threshold, const int upper_value_threshold) throw (HandGestureException) {
	if (lower_threshold < 0 || lower_threshold > 359 || upper_threshold < 0 || upper_threshold > 359)
		throw HandGestureException("Both skin hue threshold should be in [0,360)");
	if (lower_value_threshold < 0 || lower_value_threshold > 255 || upper_value_threshold < 0 || upper_value_threshold > 359)
		throw HandGestureException("Both skin value threshold should be in [0,255]");
	if (skinFilter_ && skinFilter_->getUserHueLt() == lower_threshold && skinFilter_->getUserHueUp() == upper_threshold &&
			skinFilter_->getUserValueLt() == lower_value_threshold && skinFilter_->getUserValueUt() == upper_value_threshold)
		return;
	if (skinFilter_) {
		delete skinFilter_;
	}
	skinFilter_ = new SkinDetector(skinMorphingMethod_, lower_threshold, upper_threshold, lower_value_threshold, upper_value_threshold);
}

void Control::setAdaptiveAlgoritm(const bool adapt) {
	if (skinFilter_) {
		skinFilter_->setAdapt(adapt);
	}
}

void Control::adjustTrackingSensity(const double sensity) throw (HandGestureException) {
	if (sensity < 0 || sensity > 1)
		throw HandGestureException("Tracking sensity havto be between 0 and 1");
	if (trackingSensity_ != sensity) {
		trackingSensity_ = sensity;
		for (size_t i=0; i<handTrackers_.size(); i++) {
			handTrackers_[i]->setTrackingSensity(trackingSensity_);
		}
	}
}

void Control::releaseCamera() {
	boost::mutex::scoped_lock l(frameMutex_);
	if (cap_.isOpened())
		cap_.release();
}


void Control::createIntersactionTable(int handsNum) {
	deleteIntersactionTable();
	intersactions_ = new int*[handsNum];
	for (int i=0; i<handsNum; i++) {
		intersactions_[i] = new int[handsNum];
		for (int j=0; j<handsNum; j++) {
			intersactions_[i][j] = 0;
		}
	}

}

void Control::deleteIntersactionTable() {
	if (intersactions_) {
		for (int i=0; i<handsNum_; i++) {
			if (intersactions_[i]) {
				delete [] intersactions_[i];
			}
		}
		delete [] intersactions_;
	}
	intersactions_ = NULL;
}

const vector<Mat>& Control::getTrackShapeMask() throw (HandGestureException) {
	if (!isHand_)
		throw HandGestureException("Tracking not active");
	if (isError_)
		throw HandGestureException(errorMsg_);
	boost::mutex::scoped_lock(staticImageMutex_);
	return trackShapes_;
}

void Control::enableTrajectory(bool enable) {
	trajectoryActive_ = enable;
}

const vector<Trajectory>& Control::getTrajectory(vector<bool> &trajectory_status,
												 vector<bool> &trajectory_end) const throw (HandGestureException) {
	if (!trajectoryActive_)
		throw HandGestureException("Trajectory module not activate");
	if (isError_)
		throw HandGestureException(errorMsg_);
	trajectory_end = trajectoryEnd_;
	trajectory_status = trackingState_;
	return trajectory_;
}

const vector<int>& Control::getLastDirection() const throw (HandGestureException) {
	if (!trajectoryActive_)
		throw HandGestureException("Trajecory module not activate");
	if (isError_)
		throw HandGestureException(errorMsg_);
	return lastDirection_;
}

const vector<Point>& Control::getLastPos() const throw (HandGestureException) {
	if (!trajectoryActive_)
		throw HandGestureException("Trajecory module not activate");
	if (isError_)
		throw HandGestureException(errorMsg_);
	return lastPos_;
}

void Control::processTrajectory() throw (HandGestureException) {
	for (int i=0; i<handsNum_; i++) {
		if (lastPos_[i].x >= 0) {
			trajectoryEnd_[i] = false;
			double dist = sqrt((double)(lastPos_[i].x - centers_[i].x)*(lastPos_[i].x - centers_[i].x) + (lastPos_[i].y - centers_[i].y)*(lastPos_[i].y - centers_[i].y));
			if (!trackingState_[i] && dist >= 20) {
				trackingState_[i] = true;
				trajectory_[i].data_.clear();
				trajectory_[i].type_ = "undefined";
				trajectoryEnd_[i] = false;
			}
			else if (trackingState_[i] && dist < 5) {
				trackingState_[i] = false;
				if (trajectory_[i].data_.size()>0) {
					trajectoryEnd_[i] = true;
				}
			}
			if (trackingState_[i]) {
				double angle = 0;
				int dir_code;
				int dx = centers_[i].x - lastPos_[i].x;
				int dy = - (centers_[i].y - lastPos_[i].y);

				if (dx == 0) {
					if (dy>0)
						dir_code = 4;
					else
						dir_code = 12;
				}
				else {
					angle = atan(1.f*abs(dy)/abs(dx)) + (M_PI/16);
					dir_code = (int) (angle/(M_PI/8));
					if (dx <= 0 && dy > 0) {
						dir_code = 8 - dir_code;
					}
					else if (dx < 0 && dy <= 0) {
						dir_code += 8;
					}
					else if (dx >= 0 && dy < 0) {
						dir_code = 16 - dir_code;
						dir_code %= 16;
					}
				}
				trajectory_[i].data_.push_back(dir_code);
				lastDirection_[i] = dir_code;
			}
		}
		lastPos_[i] = centers_[i];
	}
}


void Control::enableDynamicGesture(bool enable) throw (HandGestureException) {
	if (!trajectoryActive_) {
		throw HandGestureException("You have to enable trajectory tracking before enablic dynamic gestures");
	}
	dynamicGestureActive_ = enable;
}

void Control::enableStaticGesture(bool enable) {
	staticGestureActive_ = enable;
}

void Control::loadDynamicGestureModels(const vector<QString> &model_files, const QString &garbage_file) throw (HandGestureException) {
	if (!dynamic_) {
		delete dynamic_;
	}
	dynamicModelFiles_ = model_files;
	dynamicGarnageFile_ = garbage_file;
	dynamic_ = new DynamicGesture(model_files, garbage_file, handsNum_);
}

void Control::processDynamicGestures() throw (HandGestureException) {
	if (dynamic_ == NULL)
		throw HandGestureException("Dynamic gestures data model not loaded");
	dynamic_->process(trajectory_, trajectoryEnd_);
}

void Control::loadStaticGesturesModels(const QString &model_file, const QString &scale_factors_file,
										  const QString &labels_file) throw (HandGestureException) {
	if (!static_) {
		delete static_;
	}
	staticModelFile_ = model_file;
	staticScaleFactorsFile_ = scale_factors_file;
	staticLabelsFile_ = labels_file;
	static_ = new StaticGesture(model_file, scale_factors_file, labels_file, handsNum_);
}

void Control::proccessStaticGestures() throw (HandGestureException) {
	if (static_ == NULL)
		throw HandGestureException("Static data model not loaded");

	static_->proccess(getTrackShapeMask());
}

void Control::captureHandsShape() {	
	vector<Mat> shapes_tmp;
	for (int i=0; i<handsNum_; i++) {
		Mat tmp = bitMask_.colRange(handsRect_[i].x, handsRect_[i].x+handsRect_[i].width);
		tmp = tmp.rowRange(handsRect_[i].y, handsRect_[i].y+handsRect_[i].height);
		tmp = tmp>0;
		vector<vector<Point> > contours, contour_draw;
		findContours(tmp, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
		int max_area = 0;
		int max_area_id = -1;
		for (size_t i=0; i<contours.size(); i++) {
			int area = contourArea(Mat(contours[i]));
			if (area>max_area) {
				max_area = area;
				max_area_id = i;
			}
		}
		tmp = Mat::zeros(tmp.rows, tmp.cols, CV_8UC1);
		if (max_area_id != -1) {
			contour_draw.push_back(contours[max_area_id]);
			drawContours(tmp, contour_draw, -1, Scalar(255), CV_FILLED);
		}
		erode(tmp, tmp, Mat(), Point(-1,-1), 2);
		shapes_tmp.push_back(tmp);
	}
	staticImageMutex_.lock();
	trackShapes_ = shapes_tmp;
	staticImageMutex_.unlock();
}

void Control::checkIntersactions() throw(HandLostException) {
	for (int i=0; i<handsNum_; i++) {
		for (int j=0; j<handsNum_; j++) {
			if (i==j)
				break;
			Rect rect_intersect = handsRect_[i] & handsRect_[j];
			if (1. * rect_intersect.area() / handsRect_[i].area() > 0.7) {
				intersactions_[i][j]++;
				if (intersactions_[i][j] > 10) {
					throw HandLostException("Hands Tracker lost");
				}
			}
			else {
				intersactions_[i][j] = 0;
			}
		}
	}
}

void Control::stopProcess() {
	isFrame_ = false;
	isRunningMutex_.lock();
	isRunning_ = false;
	isRunningMutex_.unlock();
}

Control::ControlThread::ControlThread(Control *control) : control_(control) {}

void Control::ControlThread::operator()() {
	control_->isRunningMutex_.lock();
	control_->isRunning_ = true;
	control_->isRunningMutex_.unlock();
	bool state = true;
	while(state) {
		if(!control_->process())
			break;
		control_->isRunningMutex_.lock();
		state = control_->isRunning_;
		control_->isRunningMutex_.unlock();
		boost::this_thread::sleep(boost::posix_time::milliseconds(10));
	}
}

boost::thread Control::startThread() {
	isFrame_ = false;
	ControlThread thread(this);
	return boost::thread(thread);
}
